﻿using System;

namespace Blue_Rogue_Remote
{
    /// <summary>
    /// Tools
    /// </summary>
    public class Tools
    {
        #region members
        /// <summary>
        /// Converts and int value to a char array as the binary value
        /// </summary>
        /// <param name="x">value to convert to binary</param>
        /// <returns>char array containing binary value of input int</returns>
        public char[] int_to_binary(int x)
        {
            int cnt, mask = 1 << 31;
            char[] bitpattern = new char[32];
            for (cnt = 0; cnt <= 31; ++cnt)
            {
                bitpattern[cnt] = ((x & mask) == 0) ? '0' : '1';
                x <<= 1;
            }


            char[] nipple = new char[3];

            for (int i = 0; i < 3; i++)
            {
                nipple[i] = bitpattern[29 + i];
            }

            //System.Diagnostics.Debug.WriteLine("Bits: " + new String(bitpattern));

            return nipple;
        }

        /// <summary>
        /// Converts two int values to one byte
        /// </summary>
        /// <param name="turnrate">Amount to turn</param>
        /// <param name="speed">Amount to move</param>
        /// <returns>byte</returns>
        public byte MovementToByte(int turnrate, int speed)
        {
            //Boundries...
            turnrate = (turnrate < -7) ? -7 : turnrate;
            turnrate = (turnrate > 7) ? 7 : turnrate;
            speed = (speed < -7) ? -7 : speed;
            speed = (speed > 7) ? 7 : speed;

            //Nibbles used for construction of move byte for robot            
            char[] moveNibble = new char[4]; //Lower nibble
            char[] turnNibble = new char[4]; //Upper nibble

            //Amount of movement as binary
            char[] move = int_to_binary(Math.Abs(speed));
            char[] turn = int_to_binary(Math.Abs(turnrate));

            //construction of nibbles
            for (int i = 0; i < 3; i++)
            {
                moveNibble[1 + i] = move[i];
                turnNibble[1 + i] = turn[i];
            }

            //Sets signs on the nibbles
            moveNibble[0] = (speed < 0) ? '1' : '0';
            turnNibble[0] = (turnrate < 0) ? '1' : '0';

            //construction of whole byte to write, as a char array
            char[] writeByte = new char[8];
            for (int i = 0; i < 4; i++)
            {
                writeByte[i] = turnNibble[i];
                writeByte[4 + i] = moveNibble[i];
            }

            //Converting char array to a single byte
            byte data = Convert.ToByte(new String(writeByte), 2);

            //returns converted byte
            return data;
        }

        #endregion
    }
}
